#include "depthdataprocessor.h"
#include "PointCloudGenerator.h"
#include <QDebug>
#include <QImage>
#include <QThread>
#include <QFile>
#include <QHash>
#include <QVariant>
#include <QCoreApplication>
#include <QDateTime>
#include <QtConcurrent/QtConcurrent>  // 引入并行处理头文件
#include <numeric>  // 用于std::iota

void DepthDataProcessor::parseDepthDataUint16(const QByteArray& data, QImage& image, int width, int height) {
    const unsigned short* rawData = reinterpret_cast<const unsigned short*>(data.constData());
    int dataSize = data.size() / 2; // uint16是2字节

    // 计算最大深度值用于归一化
    unsigned short maxDepth = 0;
    for (int i = 0; i < qMin(width * height, dataSize); i++) {
        if (rawData[i] > maxDepth) {
            maxDepth = rawData[i];
        }
    }

    // 直接访问图像内存，避免setPixel的性能开销
    for (int y = 0; y < height; y++) {
        uchar* line = image.scanLine(y);
        for (int x = 0; x < width; x++) {
            int index = y * width + x;
            if (index < dataSize) {
                unsigned short depth = rawData[index];
                // 将深度值映射到RGB颜色（0=黑色，255=白色）
                int gray = static_cast<int>(depth * 255.0 / maxDepth);
                line[x * 3] = gray;     // R
                line[x * 3 + 1] = gray; // G
                line[x * 3 + 2] = gray; // B
            }
        }
    }
}



DepthDataProcessor::DepthDataProcessor(QObject* parent)
    : QObject(parent)
{
}


const QByteArray headBytes("\xAA\x55\xAA\x55\xAA\x55\xAA\x55");
const QByteArray tailBytes("\xEE\xFF\xEE\xFF");
void DepthDataProcessor::processDepthFrame(QSharedPointer<QByteArray> data)
{
#if 1
    qDebug() << "DepthDataProcessor::processDepthFrame" << QThread::currentThreadId();
    QByteArray temByteArr = *data;

    mRevBuf.append(temByteArr);
    int bufLen = mRevBuf.length();
    while (1)
    {
        int headIndex = mRevBuf.indexOf(headBytes);

        if (headIndex >= 0)
        {
            if (headIndex + 4 < bufLen)
            {
                int frameLen = *(int*)(mRevBuf.mid(headIndex + 9, 4).data());

                if (frameLen > 0)
                {
                    int tailIndex = mRevBuf.indexOf(tailBytes, headIndex + 9);

                    if (tailIndex > 0)
                    {
                        /* 完整的一帧数据 */
                        QByteArray frameArray = mRevBuf.mid(headIndex, tailIndex + 4 - headIndex - 1);

                        processCompleteFrame(frameArray);

                        /* 删除帧尾前的数据 */
                        mRevBuf.remove(0, tailIndex + 4 - 1);
                    }
                    else
                    {
                        /* 帧未收全,删除帧头前的数据 */
                        mRevBuf.remove(0, headIndex);
                        break;
                    }
                }
                else
                {
                    /* 帧长度出错 */
                    int nextHeadIndex = mRevBuf.indexOf(headBytes, headIndex + 9);
                    /* 帧尾错误,删除帧尾前的数据 */
                    if (nextHeadIndex < 0)
                    {
                        /* 下一帧头没有找到 */
                        mRevBuf.clear();
                        break;
                    }
                    else
                    {
                        /* 下一帧头前的数据 */
                        mRevBuf.remove(0, nextHeadIndex);
                    }
                }
            }
        }
        else
        {
            /* 缓冲包中不包含帧头,全清*/
            mRevBuf.clear();
            break;
        }
    }
#endif
}

/**
 * @brief 将十六进制毫秒时间戳转换为可读时间字符串
 * @param hexMsec 十六进制毫秒时间戳（如 "668344B597010000"）
 * @return 格式化后的时间字符串（yyyy-MM-dd hh:mm:ss.zzz）
 */
QString DepthDataProcessor::convertHexMsecToDateTime(const QString& hexMsec) {
    // 检查输入有效性
    if (hexMsec.length() % 2 != 0) {
        return "Invalid hex length, must be even";
    }

    // 1. 倒序十六进制字符串（按字节倒序）
    QByteArray byteArray = QByteArray::fromHex(hexMsec.toUtf8());
    std::reverse(byteArray.begin(), byteArray.end());
    QString reversedHex = QString(byteArray.toHex(' ').replace(" ", ""));
    qDebug() << "Reversed hex:" << reversedHex;

    // 2. 转换为毫秒数值
    bool ok = false;
    quint64 msecValue = reversedHex.toULongLong(&ok, 16);
    if (!ok) {
        return "Failed to convert hex to number";
    }
    qDebug() << "Milliseconds:" << msecValue;

    // 3. 转换为秒（毫秒 ÷ 1000）
    quint64 secValue = msecValue / 1000;
    quint64 msecPart = msecValue % 1000; // 提取毫秒部分

    // 4. 转换为QDateTime
    QDateTime datetime = QDateTime::fromSecsSinceEpoch(secValue);
    if (!datetime.isValid()) {
        return "Invalid timestamp value";
    }

    // 5. 格式化输出（包含毫秒部分）
    return datetime.toString("yyyy-MM-dd hh:mm:ss.") +
        QString::number(msecPart, 10).rightJustified(3, '0');
}

void DepthDataProcessor::processCompleteFrame(QByteArray& depthData)
{

    char* pData = depthData.data();

    int depthWidth = *(int*)(pData + 32 + 1);
    int depthHeight = *(int*)(pData + 32 + 5);

    int depthSize = *(qint64*)(pData + 32 + 17);

    const int dataOffset = 32 + 25;
    const int dataLength = depthWidth * depthHeight * sizeof(quint16);

    // 确保数据长度合法
    if (depthData.size() < dataOffset + dataLength) {
        qDebug() << "数据长度不足，无法解析深度数据";
        return;
    }

    // 手动复制数据到m_irData
    m_irData.resize(depthWidth * depthHeight);
    memcpy(m_irData.data(), pData + dataOffset, dataLength);

    // 创建灰度图
    QImage depthImage(depthWidth, depthHeight, QImage::Format_RGB888);
    unsigned short maxDepth = 0;
    int size = qMin(depthWidth * depthHeight, depthSize);

    // 计算最大深度值（用于归一化）
    for (int i = 0; i < size; i++) {
        unsigned short depth = m_irData[i];
        if (depth > maxDepth) {
            maxDepth = depth;
        }
    }

    // 生成RGB灰度图
    for (int y = 0; y < depthHeight; y++) {
        uchar* line = depthImage.scanLine(y);
        for (int x = 0; x < depthWidth; x++) {
            int index = y * depthWidth + x;
            if (index < size) {
                unsigned short depth = m_irData[index];
                int gray = static_cast<int>(depth * 255.0 / maxDepth);
                line[x * 3] = gray;     // R
                line[x * 3 + 1] = gray; // G
                line[x * 3 + 2] = gray; // B
            }
        }
    }

    emit depthImageReady(depthImage, m_irData, depthWidth, depthHeight);

}

QVector<quint16> DepthDataProcessor::hexStringToDepthArray(const QString& hexString, int width, int height)
{
    // 计算像素总数和期望的十六进制字符串长度
    int pixelCount = width * height;
    int expectedHexLength = pixelCount * 4; // 每个quint16需要4个十六进制字符

    // 快速验证输入长度
    if (hexString.length() != expectedHexLength) {
        qDebug() << "十六进制字符串长度错误! 期望:" << expectedHexLength << " 实际:" << hexString.length();
        return QVector<quint16>(); // 返回空数组
    }
    // 转换十六进制字符串为字节数组（每个quint16对应2字节）
    QByteArray byteArray = QByteArray::fromHex(hexString.toUtf8());
    if (byteArray.size() != pixelCount * 2) {
        qDebug() << "字节数组转换失败，长度不匹配。期望:" << pixelCount * 2 << " 实际:" << byteArray.size();
        return QVector<quint16>(); // 返回空数组
    }

    // 转换字节数组为uchar指针，便于直接访问
    const uchar* hexData = reinterpret_cast<const uchar*>(byteArray.constData());

    // 创建像素索引列表（预分配内存提高性能）
    QVector<int> pixelIndices;
    pixelIndices.reserve(pixelCount);
    for (int i = 0; i < pixelCount; ++i) {
        pixelIndices.append(i);
    }

    // 使用阻塞式并行计算（直接返回结果）
    QVector<quint16> depthValues = QtConcurrent::blockingMapped(pixelIndices, [=](int pixelIndex) {
        // 计算字节偏移（小端序处理：低字节在前，高字节在后）
        int byteOffset = pixelIndex * 2;
        quint8 lowByte = hexData[byteOffset];
        quint8 highByte = hexData[byteOffset + 1];
        return (quint16)(highByte << 8 | lowByte);
        });

    // 验证结果长度
    if (depthValues.size() == pixelCount) {
        return depthValues;
    }
    else {
        qDebug() << "并行计算结果长度不匹配，期望:" << pixelCount << " 实际:" << depthValues.size();
        return QVector<quint16>(); // 返回空数组
    }
}



quint32 DepthDataProcessor::convertHexStringToDecimal(const QString& hexStr)
{
    QByteArray byteArray = QByteArray::fromHex(hexStr.toUtf8());
    QByteArray reversedBytes = byteArray;
    std::reverse(reversedBytes.begin(), reversedBytes.end());

    quint32 result = 0;
    for (char byte : reversedBytes) {
        result = (result << 8) | static_cast<quint8>(byte);
    }
    return result;
}

QByteArray DepthDataProcessor::hexStringToLittleEndian(const QString& hexString)
{
    QByteArray byteArray = QByteArray::fromHex(hexString.toUtf8());
    std::reverse(byteArray.begin(), byteArray.end());
    return byteArray;
}


